#include <ros/ros.h> 
#include <std_msgs/String.h> 
#include "serialport/ALL_UAVs_LLA.h"
#include "geographic_msgs/GeoPoint.h"
#include <std_msgs/Bool.h>
#include <std_msgs/UInt8.h>
using namespace std;

void uavs_lla_cb(const serialport::ALL_UAVs_LLA::ConstPtr& msg)
{
    for(uint8_t i=0;i<4;i++)
    {
        cout  << "UAV" << i+1 << ":" 
                    << msg->all_uavs_lla[i].longitude << "\t" 
                    << msg->all_uavs_lla[i].latitude << "\t" 
                    << msg->all_uavs_lla[i].altitude 
                    << endl;
    }
}
void takeoff_flag_cb(const std_msgs::Bool::ConstPtr& msg)
{
    // cout << "takeoff_flag:" << msg->data << endl;
    printf("%#x\n",msg->data);
}

int num = 0;
int main(int argc, char **argv)
{
    ros::init(argc, argv, "listener");
    ros::NodeHandle nh;
    ros::Rate loop_rate(20);                            //设置循环频率，括号内参数即为频率
    // 【订阅】无人机的位置消息
    ros::Subscriber uavs_lla_sub = nh.subscribe<serialport::ALL_UAVs_LLA>("/all_uavs_position/global_lcy", 10, uavs_lla_cb);
    // 【订阅】无人机的起飞状态
    ros::Subscriber takeoff_flag_sub = nh.subscribe<std_msgs::Bool>("/px4_command/takeoff_flag", 10, takeoff_flag_cb);

    while(ros::ok())
    {
        //执行回调函数
        // ros::spinOnce();
        // num++;
        // cout << "num:" << num << endl;
        // cout << endl;
        ros::spin();
        loop_rate.sleep();
    }
    return 0;
}
